import time
import cv2
import threading
import numpy as np
import rospy
import math
from functools import wraps
import sys

from image_Tag_converter import ImgConverter
from image_Tag_converter import TagConverter

sys.path.append("/home/lemon/catkin_ws/src/aelos_smart_ros")
from leju import base_action


#定义一些参数
Chest_img = None
ChestOrg = None

marker = None

chest_circle_x = None
chest_circle_y = None
Debug = 0

# 不同色块的hsv范围
color_range = {
    'green': [( 49 , 115 , 80 ),( 83 , 183 , 162 )],
    'orange': [( 49 , 115 , 80 ),( 83 , 183 , 162 )]
}



# ******************************************动作函数***********************************************
def box_go(n):
    for i in range (0,n):
        base_action.action("BoxForward4s_5")
        time.sleep(0.5)
def box_go1(n):    #抱着箱子前进一步
    for i in range (0,n):
        base_action.action("boxForward8s_B081_5")
        time.sleep(0.3)
def box_go2(n):     #抱着箱子前进两步
    for i in range (0,n):
        base_action.action("BoxForward2s")
        time.sleep(0.5)
def box_go3(n):     #抱着箱子前进三步
    for i in range (0,n):
        base_action.action("BoxForward3s")
        time.sleep(0.5)
def box_go4(n):     #抱着箱子前进三步
    for i in range (0,n):
        base_action.action("BoxForward4s")
        time.sleep(0.5)
def box_go5(n):     #抱着箱子前进三步
    for i in range (0,n):
        base_action.action("BoxForward5s")
        time.sleep(0.5)
def Box_go_slow4s (n):           #右转
    for i in  range (0,n):
        base_action.action("boxForwardSlow4s_3")
        time.sleep(0.5)
def Box_go_slow6s (n):           #右转
    for i in  range (0,n):
        base_action.action("boxForwardSlow6s_1")
        time.sleep(0.5)
def Box_go_slow8s (n):           #右转
    for i in  range (0,n):
        base_action.action("boxForwardSlow8s_1")
        time.sleep(0.5)
def Box_Back1 (n):           #右转
    for i in  range (0,n):
        base_action.action("boxForwardSlow8s_1")
        time.sleep(0.5)

if __name__ == '__main__':
   print("指令集如下：")
   print(" 0 ---> box_go ---> 抱箱慢走前进")
   print(" 1 ---> box_go1 ---> 抱箱快走前进1")
   print(" 2 ---> box_go2 ---> 抱箱快走前进2")
   print(" 3 ---> box_go3 ---> 抱箱快走前进3")
   print(" 4 ---> box_go4 ---> 抱箱快走前进3")
   print(" 5 ---> box_go5 ---> 抱箱快走前进3")
   print(" 6 ---> box_go_slow4s ---> 抱箱慢走前进4s")
   print(" 7 ---> box_go_slow6s ---> 抱箱慢走前进6s")
   print(" 8 ---> box_go_slow8s ---> 抱箱慢走前进8s")
   print(" 9 ---> box_back_run ---> 抱箱后退")


   while True:
    key=input("请输入控制指令：")
    if(key=="0"):
        actionNumber=int(input("请输入执行次数："))
        box_go(actionNumber)
        print("box_go")
    elif(key=="1"):
        actionNumber=int(input("请输入执行次数："))
        box_go1(actionNumber)
        print("box_go1")
    elif(key=="2"):
        actionNumber=int(input("请输入执行次数："))
        box_go2(actionNumber)
        print("box_go2")
    elif(key=="3"):
        actionNumber=int(input("请输入执行次数："))
        box_go3(actionNumber)
        print("box_go3")
    elif(key=="4"):
        actionNumber=int(input("请输入执行次数："))
        box_go4(actionNumber)
        print("box_go4")
    elif(key=="5"):
        actionNumber=int(input("请输入执行次数："))
        box_go5(actionNumber)
        print("box_go5")
    elif(key=="6"):
        actionNumber=int(input("请输入执行次数："))
        Box_go_slow4s(actionNumber)
    elif(key=="7"):
        actionNumber=int(input("请输入执行次数："))
        Box_go_slow6s(actionNumber)
    elif(key=="8"):
        actionNumber=int(input("请输入执行次数："))
        Box_go_slow8s(actionNumber)
    elif(key=="9"):
        actionNumber=int(input("请输入执行次数："))
        Box_Back1(actionNumber)
